//
//  util.cpp
//  BlueBeacon
//
//  Created by INNO on 13-12-24.
//  Copyright (c) 2013年 yy. All rights reserved.
//

#include "util.h"

//输出
void printf_matrix(float *m)
{
    int i = 0;
    int j = 0;
    
    for (i = 0;i < 3;i++)
    {
        for (j = 0;j < 3;j++)
        {
            printf("%f\t",*(m + i * 3 + j));
            
            //qDebug() << *(m + i * 3 + j);
        }
    }
}

//三维行列式的值
//m:3 * 3数组
double det(float *m)
{
    double value = 0.0;
    
    value = (m + 0)[0] * (m + 3)[1] * (m + 6)[2] + \
    (m + 0)[1] * (m + 3)[2] * (m + 6)[0] + \
    (m + 0)[2] * (m + 3)[0] * (m + 6)[1] - \
    (m + 0)[1] * (m + 3)[0] * (m + 6)[2] - \
    (m + 0)[2] * (m + 3)[1] * (m + 6)[0] - \
    (m + 0)[0] * (m + 3)[2] * (m + 6)[1];
    
    return value;
}

//将一个行列式的值赋给另一个
//src,dst:3 * 3数组
void copy_matrix(float *src,float *dst)
{
    int i = 0;
    int j = 0;
    
    for (i = 0;i < 3;i++)
    {
        for (j = 0;j < 3;j++)
        {
            *(dst + i * 3 + j) = *(src + i * 3 + j);
        }
    }
}

//解方程
//m:方阵,3 * 3数组
//b:解
//fail:back -1
int solve(float *m,float *b,float *x)
{
    float det_m;
    float det_m_temp;
    float m_temp[3][3];
    int i = 0;
    int j = 0;
    
    det_m = det(m);
    if (det_m == 0)
    {
        return -1;
    }
    for (j = 0;j < 3;j++)
    {
        //得到新的行列式
        copy_matrix(m,m_temp[0]);
        for (i = 0;i < 3;i++)
        {
            m_temp[i][j] = *(b + i);
        }
        det_m_temp = det(m_temp[0]);
        
        //求解
        x[j] = det_m_temp / det_m;
    }
    
    return 0;
}

//计算空间点到原点距离的平方
float d_p_square(float *p)
{
    float d = 0;
    int i = 0;
    
    for (i = 0;i < 3;i++)
    {
        d += (*(p + i)) * (*(p + i));
    }
    
    return d;
}